//////////////////////////////////////////////////////////////////////////
#pragma once


#include "Math/Quaternionf.hpp"


namespace Math
{
	//////////////////////////////////////////////////////////////////////////
	inline Matrix44f::Matrix44f()
	{
	}


	inline Matrix44f::Matrix44f( const Matrix44f& rhs )
	{
		mColumns[ 0 ]	=	rhs.mColumns[ 0 ];
		mColumns[ 1 ]	=	rhs.mColumns[ 1 ];
		mColumns[ 2 ]	=	rhs.mColumns[ 2 ];
		mColumns[ 3 ]	=	rhs.mColumns[ 3 ];
	}


	inline Matrix44f::Matrix44f(
		float	m11,	float	m21,	float	m31,	float	m41,
		float	m12,	float	m22,	float	m32,	float	m42,
		float	m13,	float	m23,	float	m33,	float	m43,
		float	m14,	float	m24,	float	m34,	float	m44
		)
	{
		mColumns[ 0 ].Set( m11, m12, m13, m14 );
		mColumns[ 1 ].Set( m21, m22, m23, m24 );
		mColumns[ 2 ].Set( m31, m32, m33, m34 );
		mColumns[ 3 ].Set( m41, m42, m43, m44 );
	}


	inline Matrix44f::Matrix44f(
		const Vector4f&	column1,
		const Vector4f&	column2,
		const Vector4f&	column3,
		const Vector4f&	column4
		)
	{
		mColumns[ 0 ]	=	column1;
		mColumns[ 1 ]	=	column2;
		mColumns[ 2 ]	=	column3;
		mColumns[ 3 ]	=	column4;
	}


	//////////////////////////////////////////////////////////////////////////
	inline Vector4f& Matrix44f::operator()( size_t column )
	{
		return mColumns[ column ];
	}


	inline const Vector4f& Matrix44f::operator()( size_t column ) const
	{
		return mColumns[ column ];
	}


	inline void Matrix44f::operator()( size_t column, const Vector4f& v )
	{
		mColumns[ column ] = v;
	}


	//////////////////////////////////////////////////////////////////////////
	inline const Matrix44f& Matrix44f::operator =( const Matrix44f& rhs )
	{
		mColumns[ 0 ]	=	rhs.mColumns[ 0 ];
		mColumns[ 1 ]	=	rhs.mColumns[ 1 ];
		mColumns[ 2 ]	=	rhs.mColumns[ 2 ];
		mColumns[ 3 ]	=	rhs.mColumns[ 3 ];
		return *this;
	}


	//////////////////////////////////////////////////////////////////////////
	inline const Matrix44f Matrix44f::operator *( const Matrix44f& rhs ) const
	{
		Matrix44f	result	=	Matrix44f::Zero();
		for( int i=0; i<4; ++i )
			for( int j=0; j<4; ++j )
				for( int k=0; k<4; ++k )
					result( i )( j, result( i )( j ) + (*this)( i )( k ) * rhs( k )( j ) );
		return result;
	}

	inline const Vector4f Matrix44f::operator *( const Vector4f& rhs ) const
	{
		Vector4f	result(
			mColumns[ 0 ].X() * rhs( 0 ) + mColumns[ 1 ].X() * rhs( 1 ) + mColumns[ 2 ].X() * rhs( 2 ) + mColumns[ 3 ].X() * rhs( 3 ),
			mColumns[ 0 ].Y() * rhs( 0 ) + mColumns[ 1 ].Y() * rhs( 1 ) + mColumns[ 2 ].Y() * rhs( 2 ) + mColumns[ 3 ].Y() * rhs( 3 ),
			mColumns[ 0 ].Z() * rhs( 0 ) + mColumns[ 1 ].Z() * rhs( 1 ) + mColumns[ 2 ].Z() * rhs( 2 ) + mColumns[ 3 ].Z() * rhs( 3 ),
			mColumns[ 0 ].W() * rhs( 0 ) + mColumns[ 1 ].W() * rhs( 1 ) + mColumns[ 2 ].W() * rhs( 2 ) + mColumns[ 3 ].W() * rhs( 3 )
			);
		return result;
	}

	inline void Matrix44f::operator *=( float rhs )
	{
		mColumns[ 0 ]	*=	rhs;
		mColumns[ 1 ]	*=	rhs;
		mColumns[ 2 ]	*=	rhs;
		mColumns[ 3 ]	*=	rhs;
	}


	//////////////////////////////////////////////////////////////////////////
	inline void Matrix44f::Transpose()
	{
		mColumns[ 0 ]	=	Vector4f( mColumns[ 0 ].X(), mColumns[ 1 ].X(), mColumns[ 2 ].X(), mColumns[ 3 ].X() );
		mColumns[ 1 ]	=	Vector4f( mColumns[ 0 ].Y(), mColumns[ 1 ].Y(), mColumns[ 2 ].Y(), mColumns[ 3 ].Y() );
		mColumns[ 2 ]	=	Vector4f( mColumns[ 0 ].Z(), mColumns[ 1 ].Z(), mColumns[ 2 ].Z(), mColumns[ 3 ].Z() );
		mColumns[ 3 ]	=	Vector4f( mColumns[ 0 ].W(), mColumns[ 1 ].W(), mColumns[ 2 ].W(), mColumns[ 3 ].W() );
	}


	inline void Matrix44f::Inverse()
	{
		// Cramer's rule inversion.
		// Modified from: ftp://download.intel.com/design/PentiumIII/sml/24504301.pdf
		float		temp[ 12 ];
		float		trans[ 16 ];

		// Transpose this into a linear array.
		// TODO: this can be replaced by correcting the indexing into 'this'.
		for( int i=0; i<4; ++i )
		{
			trans[ i ]		=	mColumns[ i ]( 0 );
			trans[ i + 4 ]	=	mColumns[ i ]( 1 );
			trans[ i + 8 ]	=	mColumns[ i ]( 2 );
			trans[ i + 12 ]	=	mColumns[ i ]( 3 );
		}

		// Compute first set of cofactors.
		temp[ 0  ]	=	trans[ 10 ]	*	trans[ 15 ];
		temp[ 1  ]	=	trans[ 11 ]	*	trans[ 14 ];
		temp[ 2  ]	=	trans[ 9  ]	*	trans[ 15 ];
		temp[ 3  ]	=	trans[ 11 ]	*	trans[ 13 ];
		temp[ 4  ]	=	trans[ 9  ]	*	trans[ 14 ];
		temp[ 5  ]	=	trans[ 10 ]	*	trans[ 13 ];
		temp[ 6  ]	=	trans[ 8  ]	*	trans[ 15 ];
		temp[ 7  ]	=	trans[ 11 ]	*	trans[ 12 ];
		temp[ 8  ]	=	trans[ 8  ]	*	trans[ 14 ];
		temp[ 9  ]	=	trans[ 10 ]	*	trans[ 12 ];
		temp[ 10 ]	=	trans[ 8  ]	*	trans[ 13 ];
		temp[ 11 ]	=	trans[ 9  ]	*	trans[ 12 ];

		// First 8 elements.
		(*this)( 0 )( 0, (temp[ 0 ]*trans[ 5 ] + temp[ 3 ]*trans[ 6 ] + temp[ 4 ]*trans[ 7 ])	-	(temp[ 1 ]*trans[ 5] + temp[ 2 ]*trans[ 6 ] + temp[ 5]*trans[ 7 ]) );
		(*this)( 0 )( 1, (temp[ 1 ]*trans[ 4 ] + temp[ 6 ]*trans[ 6 ] + temp[ 9 ]*trans[ 7 ])	-	(temp[ 0 ]*trans[ 4] + temp[ 7 ]*trans[ 6 ] + temp[ 8]*trans[ 7 ]) );
		(*this)( 0 )( 2, (temp[ 2 ]*trans[ 4 ] + temp[ 7 ]*trans[ 5 ] + temp[ 10]*trans[ 7 ])	-	(temp[ 3 ]*trans[ 4] + temp[ 6 ]*trans[ 5 ] + temp[11]*trans[ 7 ]) );
		(*this)( 0 )( 3, (temp[ 5 ]*trans[ 4 ] + temp[ 8 ]*trans[ 5 ] + temp[ 11]*trans[ 6 ])	-	(temp[ 4 ]*trans[ 4] + temp[ 9 ]*trans[ 5 ] + temp[10]*trans[ 6 ]) );
		(*this)( 1 )( 0, (temp[ 1 ]*trans[ 1 ] + temp[ 2 ]*trans[ 2 ] + temp[ 5 ]*trans[ 3 ])	-	(temp[ 0 ]*trans[ 1] + temp[ 3 ]*trans[ 2 ] + temp[ 4]*trans[ 3 ]) );
		(*this)( 1 )( 1, (temp[ 0 ]*trans[ 0 ] + temp[ 7 ]*trans[ 2 ] + temp[ 8 ]*trans[ 3 ])	-	(temp[ 1 ]*trans[ 0] + temp[ 6 ]*trans[ 2 ] + temp[ 9]*trans[ 3 ]) );
		(*this)( 1 )( 2, (temp[ 3 ]*trans[ 0 ] + temp[ 6 ]*trans[ 1 ] + temp[ 11]*trans[ 3 ])	-	(temp[ 2 ]*trans[ 0] + temp[ 7 ]*trans[ 1 ] + temp[10]*trans[ 3 ]) );
		(*this)( 1 )( 3, (temp[ 4 ]*trans[ 0 ] + temp[ 9 ]*trans[ 1 ] + temp[ 10]*trans[ 2 ])	-	(temp[ 5 ]*trans[ 0] + temp[ 8 ]*trans[ 1 ] + temp[11]*trans[ 2 ]) );

		// Compute second set of cofactors.
		temp[ 0  ]	=	trans[ 2 ]	*	trans[ 7 ];
		temp[ 1  ]	=	trans[ 3 ]	*	trans[ 6 ];
		temp[ 2  ]	=	trans[ 1 ]	*	trans[ 7 ];
		temp[ 3  ]	=	trans[ 3 ]	*	trans[ 5 ];
		temp[ 4  ]	=	trans[ 1 ]	*	trans[ 6 ];
		temp[ 5  ]	=	trans[ 2 ]	*	trans[ 5 ];
		temp[ 6  ]	=	trans[ 0 ]	*	trans[ 7 ];
		temp[ 7  ]	=	trans[ 3 ]	*	trans[ 4 ];
		temp[ 8  ]	=	trans[ 0 ]	*	trans[ 6 ];
		temp[ 9  ]	=	trans[ 2 ]	*	trans[ 4 ];
		temp[ 10 ]	=	trans[ 0 ]	*	trans[ 5 ];
		temp[ 11 ]	=	trans[ 1 ]	*	trans[ 4 ];

		// Second set of 8 elements.
		(*this)( 2 )( 0, (temp[ 0 ]*trans[13] + temp[3 ]*trans[14] + temp[4 ]*trans[15])	-	(temp[1 ]*trans[13] + temp[2 ]*trans[14] + temp[5 ]*trans[15]) );
		(*this)( 2 )( 1, (temp[ 1 ]*trans[12] + temp[6 ]*trans[14] + temp[9 ]*trans[15])	-	(temp[0 ]*trans[12] + temp[7 ]*trans[14] + temp[8 ]*trans[15]) );
		(*this)( 2 )( 2, (temp[ 2 ]*trans[12] + temp[7 ]*trans[13] + temp[10]*trans[15])	-	(temp[3 ]*trans[12] + temp[6 ]*trans[13] + temp[11]*trans[15]) );
		(*this)( 2 )( 3, (temp[ 5 ]*trans[12] + temp[8 ]*trans[13] + temp[11]*trans[14])	-	(temp[4 ]*trans[12] + temp[9 ]*trans[13] + temp[10]*trans[14]) );
		(*this)( 3 )( 0, (temp[ 2 ]*trans[10] + temp[5 ]*trans[11] + temp[1 ]*trans[9 ])	-	(temp[4 ]*trans[11] + temp[0 ]*trans[9 ] + temp[3 ]*trans[10]) );
		(*this)( 3 )( 1, (temp[ 8 ]*trans[11] + temp[0 ]*trans[8 ] + temp[7 ]*trans[10])	-	(temp[6 ]*trans[10] + temp[9 ]*trans[11] + temp[1 ]*trans[8 ]) );
		(*this)( 3 )( 2, (temp[ 6 ]*trans[9 ] + temp[11]*trans[11] + temp[3 ]*trans[8 ])	-	(temp[10]*trans[11] + temp[2 ]*trans[8 ] + temp[7 ]*trans[9 ]) );
		(*this)( 3 )( 3, (temp[ 10]*trans[10] + temp[4 ]*trans[8 ] + temp[9 ]*trans[9 ])	-	(temp[8 ]*trans[9 ] + temp[11]*trans[10] + temp[5 ]*trans[8 ]) );

		// Compute reciprocal determinant and apply to matrix.
		float	det	=	1.0f / (trans[0]*(*this)( 0 )( 0 )+trans[1]*(*this)( 0 )( 1 )+trans[2]*(*this)( 0 )( 2 )+trans[3]*(*this)( 0 )( 3 ));
		(*this)	*=	det;
	}


	//////////////////////////////////////////////////////////////////////////
	inline Matrix44f Matrix44f::Zero()
	{
		return Matrix44f(
			Vector4f::Zero(),
			Vector4f::Zero(),
			Vector4f::Zero(),
			Vector4f::Zero()
			);
	}


	inline Matrix44f Matrix44f::Identity()
	{
		return Matrix44f(
			Vector4f::XAxis(),
			Vector4f::YAxis(),
			Vector4f::ZAxis(),
			Vector4f::WAxis()
			);
	};


	inline Matrix44f Matrix44f::Translation( float x, float y, float z )
	{
		return Matrix44f(
			1.0f,	0.0f,	0.0f,	x,
			0.0f,	1.0f,	0.0f,	y,
			0.0f,	0.0f,	1.0f,	z,
			0.0f,	0.0f,	0.0f,	1.0f
			);
	}
	inline Matrix44f Matrix44f::Translation( const Vector3f& t )				{return Translation( t.X(), t.Y(), t.Z() );}
	inline Matrix44f Matrix44f::Scale( float x, float y, float z )
	{
		return Matrix44f(
			x,		0.0f,	0.0f,	0.0f,
			0.0f,	y,		0.0f,	0.0f,
			0.0f,	0.0f,	z,		0.0f,
			0.0f,	0.0f,	0.0f,	1.0f
			);
	}
	inline Matrix44f Matrix44f::Scale( const Vector3f& t )						{return Scale( t.X(), t.Y(), t.Z() );}
	inline Matrix44f Matrix44f::RotateX( float angle )
	{
		float	sa	=	std::sin( angle );
		float	ca	=	std::cos( angle );

		return Matrix44f(
			1.0f,		0.0f,		0.0f,		0.0f,
			0.0f,		ca,			-sa,		0.0f,
			0.0f,		sa,			ca,			0.0f,
			0.0f,		0.0f,		0.0f,		1.0f
			);
	}
	inline Matrix44f Matrix44f::RotateY( float angle )
	{
		float	sa	=	std::sin( angle );
		float	ca	=	std::cos( angle );

		return Matrix44f(
			ca,			0.0f,		sa,			0.0f,
			0.0f,		1.0f,		0.0f,		0.0f,
			-sa,		0.0f,		ca,			0.0f,
			0.0f,		0.0f,		0.0f,		1.0f
			);
	}
	inline Matrix44f Matrix44f::RotateZ( float angle )
	{
		float	sa	=	std::sin( angle );
		float	ca	=	std::cos( angle );

		return Matrix44f(
			ca,			-sa,		0.0f,		0.0f,
			sa,			ca,			0.0f,		0.0f,
			0.0f,		0.0f,		1.0f,		0.0f,
			0.0f,		0.0f,		0.0f,		1.0f
			);
	}
	inline Matrix44f Matrix44f::Rotate( const Vector3f& axis, float angle )
	{
		assert( MagnitudeSquared( axis ) > 0.0001f );
		Vector3f	a	=	Normalize( axis );
		Vector3f	s	=	Multiply( a, a );

		float	ca	=	std::cos( angle );
		float	sa	=	std::sin( angle );

		return Matrix44f(
			s.X()+ca*(1-s.X()),								(a.X()*a.Y())+ca*(-a.X()*a.Y())+sa*(-a.Z()),	(a.X()*a.Z())+ca*(-a.X()*a.Z())+sa*a.Y(),		0,
			(a.X()*a.Y())+ca*(-a.X()*a.Y())+sa*a.Z(),		s.Y()+ca*(1-s.Y()),								(a.Y()*a.Z())+ca*(-a.Y()*a.Z())+sa*(-a.X()),	0,
			(a.X()*a.Z())+ca*(-a.X()*a.Z())+sa*(-a.Y()),	(a.Y()*a.Z())+ca*(-a.Y()*a.Z())+sa*a.X(),		s.Z()+ca*(1-s.Z()),								0,
			0,												0,												0,												1	);
	}


	inline Matrix44f Matrix44f::From( const Quaternionf& q )
	{
		float	x	=	q.X();
		float	y	=	q.Y();
		float	z	=	q.Z();
		float	w	=	q.W();
		
		return Math::Matrix44f(
			1 - ( (2*y*y) + (2*z*z) ),	2*x*y + 2*z*w,				2*x*z - 2*y*w,				0,
			2*x*y - 2*z*w,				1 - ( (2*x*x) + (2*z*z) ),	2*y*z + 2*x*w,				0,
			2*x*z + 2*y*w,				2*y*z - 2*x*w,				1 - ( (2*x*x) + (2*y*y) ),	0,
			0,							0,							0,							1
		);
	}

	
	//////////////////////////////////////////////////////////////////////////
	inline const Vector3f		operator *( const Math::Matrix44f& lhs, const Vector3f& rhs )
	{
		Vector4f	temp( rhs.X(), rhs.Y(), rhs.Z(), 1.0f );
		temp	=	lhs * temp;

		return Vector3f(
			temp.X(),
			temp.Y(),
			temp.Z()
			);
	}
}
